/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Autonomous extends IterativeRobot {
    
    CANJaguar conveyor;
    CANJaguar shooterA;
    CANJaguar shooterB;
    CANJaguar shooterC;
    
    Relay Solonoid = new Relay(0);
    
    Timer AutoTimer = new Timer();
         
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        try {
            CANJaguar conveyor = new CANJaguar(0);
            CANJaguar shooterA = new CANJaguar(0);
            CANJaguar shooterB = new CANJaguar(0);
            CANJaguar shooterC = new CANJaguar(0);
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
        
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        
        AutoTimer.reset();
        AutoTimer.start();
        double time = AutoTimer.get();
        
        for (int BallShot = 0;time < 14 && BallShot < 2;BallShot++) {
            System.out.print("Beginning Load " + BallShot + "\n");
            Load();
            System.out.print(BallShot + " Loaded\n");
            Shoot();
            System.out.print(BallShot + " Fired\n");
            time = AutoTimer.get();
            System.out.print("Time: " + time + "\n");
        }
        
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        
    }
    
    public void Load() {
        System.out.print("Load Started\n");
        Solonoid.set(Relay.Value.kOn);
        System.out.print("In Load: Solonoid down\n");
        try {
            conveyor.setX(0.5);
            System.out.print("In Load: Conveyor started\n");
            Timer.delay(4);
            System.out.print("In Load: Delay finished\n");
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
        System.out.print("Load Finished\n");
    }
    
    public void Shoot() {
        System.out.print("Shoot Started\n");
        try {
            shooterA.setX(1);
            shooterB.setX(1);
            shooterC.setX(1);
            System.out.print("In Shoot: Motors started\n");
            Timer.delay(2);
            System.out.print("In Shoot: Delay finished\n");
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
        Solonoid.set(Relay.Value.kOff);
        System.out.print("In Shoot: Solonoid up\n");
        System.out.print("Shoot Finished\n");
    }
    
}
